#include "sensor_bmi160.h"
#include "lib/bmi160.h"
#include "lib/bmm150.h"

#include <stm32f4xx_hal.h>
#include <sensor_common.h>

#define DBG_ENABLE
#define DBG_LEVEL DBG_LOG
#define DBG_SECTION_NAME "bmi160"
#define DBG_COLOR
#include <rtdbg.h>

#define FIFO_FRAME_CNT 8

#if defined(BMI160_USING_ACCE) || defined(BMI160_USING_GYRO) || defined(BMX160_USING_MAG)

#ifdef BMI160_USING_I2C
static I2C_HandleTypeDef bmi160_i2c_handle;
#else
static SPI_HandleTypeDef bmi160_spi_handle;
#endif

static rt_device_t bmi160_acce_dev = RT_NULL;
static rt_device_t bmi160_gyro_dev = RT_NULL;
static rt_device_t bmx160_mag_dev = RT_NULL;
static struct bmi160_dev *bmi160_device = RT_NULL;
static enum bmi160_fetch_data_mode read_mode = sensor_mode_normal;

struct odr_node
{
    rt_uint16_t odr;
    rt_uint16_t mask;
};

static rt_uint16_t range_find_mask(const struct odr_node *tab, rt_uint16_t odr)
{
    const struct odr_node *node = tab;

    for (; node->odr && node->odr <= odr; node++)
        ;
    return node->odr && node != tab ? node[-1].mask : node->mask;
}

static void _delay_ms(uint32_t period)
{
    rt_thread_mdelay(period);
}

/* device interface */
static rt_err_t rt_bmi160_acce_open(rt_device_t dev, rt_uint16_t oflag);
static rt_err_t rt_bmi160_acce_close(rt_device_t dev);
static rt_size_t rt_bmi160_acce_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size);
static rt_size_t rt_bmi160_acce_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size);

static rt_err_t rt_bmi160_control(rt_device_t dev, int cmd, void *args);

static rt_err_t rt_bmi160_gyro_open(rt_device_t dev, rt_uint16_t oflag);
static rt_err_t rt_bmi160_gyro_close(rt_device_t dev);
static rt_size_t rt_bmi160_gyro_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size);
static rt_size_t rt_bmi160_gyro_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size);

#ifdef BMI160_USING_I2C

static int8_t bmi160_i2c_msp_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    BMI160_I2C_SCL_GPIO_RCC_ENABLE();
    BMI160_I2C_SDA_GPIO_RCC_ENABLE();
    BMI160_I2C_RCC_ENABLE();

    /*##-2- Configure peripheral GPIO ##########################################*/
    /* UART TX GPIO pin configuration  */
    GPIO_InitStruct.Pin = BMI160_I2C_SCL_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = BMI160_I2C_SCL_AF;

    HAL_GPIO_Init(BMI160_I2C_SCL_GPIO, &GPIO_InitStruct);

    /* UART RX GPIO pin configuration  */
    GPIO_InitStruct.Pin = BMI160_I2C_SDA_PIN;
    GPIO_InitStruct.Alternate = BMI160_I2C_SDA_AF;

    HAL_GPIO_Init(BMI160_I2C_SDA_GPIO, &GPIO_InitStruct);

    I2C_InitTypeDef I2C_InitStruct;
    I2C_InitStruct.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
    I2C_InitStruct.ClockSpeed = 100 * 1000;
    I2C_InitStruct.DualAddressMode = I2C_DUALADDRESS_DISABLE;
    I2C_InitStruct.DutyCycle = I2C_DUTYCYCLE_2;
    I2C_InitStruct.GeneralCallMode = I2C_GENERALCALL_DISABLE;
    I2C_InitStruct.NoStretchMode = I2C_NOSTRETCH_DISABLE;
    I2C_InitStruct.OwnAddress1 = 0x01;

    bmi160_i2c_handle.Instance = BMI160_I2C;
    bmi160_i2c_handle.Init = I2C_InitStruct;
    bmi160_i2c_handle.MemaddSize = I2C_MEMADD_SIZE_8BIT;

    // HAL_I2CEx_ConfigAnalogFilter(&bmi160_i2c_handle, I2C_ANALOGFILTER_ENABLE);

    if (HAL_I2C_Init(&bmi160_i2c_handle) != HAL_OK)
        return RT_ERROR;

    __HAL_I2C_ENABLE(&bmi160_i2c_handle);

    return RT_EOK;
}

static int8_t _i2c_write_reg(void *bus, uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len)
{
    HAL_I2C_Mem_Write(&bmi160_i2c_handle, addr, reg, I2C_MEMADD_SIZE_8BIT, data, len, len);
    return RT_EOK;
}

static int8_t _i2c_read_reg(void *bus, uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len)
{
    HAL_I2C_Mem_Read(&bmi160_i2c_handle, addr, reg, I2C_MEMADD_SIZE_8BIT, data, len, len);
    return RT_EOK;
}

#else

static int8_t bmi160_spi_msp_init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;

    /*##-1- Enable peripherals Clocks #################################*/
    BMI160_SPI_SCK_GPIO_RCC_ENABLE();
    BMI160_SPI_MISO_GPIO_RCC_ENABLE();
    BMI160_SPI_MOSI_GPIO_RCC_ENABLE();
    BMI160_SPI_CS_GPIO_RCC_ENABLE();

    BMI160_SPI_RCC_ENABLE();

    /*##-2- Configure peripheral GPIO ##########################################*/
    /* UART TX GPIO pin configuration  */
    GPIO_InitStruct.Pin = BMI160_SPI_SCK_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_PULLUP;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = BMI160_SPI_SCK_AF;

    HAL_GPIO_Init(BMI160_SPI_SCK_GPIO, &GPIO_InitStruct);

    /* UART RX GPIO pin configuration  */
    GPIO_InitStruct.Pin = BMI160_SPI_MOSI_PIN;
    GPIO_InitStruct.Alternate = BMI160_SPI_MOSI_AF;

    HAL_GPIO_Init(BMI160_SPI_MOSI_GPIO, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = BMI160_SPI_MISO_PIN;
    GPIO_InitStruct.Alternate = BMI160_SPI_MISO_AF;

    HAL_GPIO_Init(BMI160_SPI_MISO_GPIO, &GPIO_InitStruct);

    GPIO_InitStruct.Pin = BMI160_SPI_CS_PIN;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    HAL_GPIO_Init(BMI160_SPI_CS_GPIO, &GPIO_InitStruct);

    bmi160_spi_handle.Instance = BMI160_SPI;
    bmi160_spi_handle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
    bmi160_spi_handle.Init.Direction = SPI_DIRECTION_2LINES;
    bmi160_spi_handle.Init.CLKPhase = SPI_PHASE_1EDGE;
    bmi160_spi_handle.Init.CLKPolarity = SPI_POLARITY_LOW;
    bmi160_spi_handle.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
    bmi160_spi_handle.Init.CRCPolynomial = 7;
    bmi160_spi_handle.Init.DataSize = SPI_DATASIZE_8BIT;
    bmi160_spi_handle.Init.FirstBit = SPI_FIRSTBIT_MSB;
    bmi160_spi_handle.Init.NSS = SPI_NSS_SOFT;
    bmi160_spi_handle.Init.TIMode = SPI_TIMODE_DISABLE;

    bmi160_spi_handle.Init.Mode = SPI_MODE_MASTER;

    if (HAL_SPI_Init(&bmi160_spi_handle) != HAL_OK)
    {
        return RT_ERROR;
    }

    __HAL_SPI_ENABLE(&bmi160_spi_handle);

    return RT_EOK;
}

static int8_t bmi160_spi_write_reg(void *bus, uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len)
{
    // 使用SPI总线，先向BMI160发送寄存器地址，然后向寄存器写入数据
    HAL_GPIO_WritePin(BMI160_SPI_CS_GPIO, BMI160_SPI_CS_PIN, RESET);
    reg |= BMI160_SPI_WR_MASK;
    HAL_SPI_Transmit(&bmi160_spi_handle, &reg, 1, 1);
    HAL_StatusTypeDef rslt = HAL_SPI_Transmit(&bmi160_spi_handle, data, len, len * 5);
    HAL_GPIO_WritePin(BMI160_SPI_CS_GPIO, BMI160_SPI_CS_PIN, SET);
    return rslt == HAL_OK ? RT_EOK : RT_ERROR;
}

static int8_t bmi160_spi_read_reg(void *bus, uint8_t addr, uint8_t reg, uint8_t *data, uint16_t len)
{
    HAL_GPIO_WritePin(BMI160_SPI_CS_GPIO, BMI160_SPI_CS_PIN, RESET);
    reg |= BMI160_SPI_RD_MASK;
    HAL_SPI_Transmit(&bmi160_spi_handle, &reg, 1, 1);
    uint8_t temp_buf[0x40]; /* 给一个较大的读取buffer */
    HAL_SPI_Receive(&bmi160_spi_handle, temp_buf, len + 1, len * 5);
    HAL_GPIO_WritePin(BMI160_SPI_CS_GPIO, BMI160_SPI_CS_PIN, SET);

    memcpy(data, &temp_buf[1], len);
    return RT_EOK;
}

#endif

#ifdef BMX160_USING_MAG
/*wrapper function to match the signature of bmm150.read */
static int8_t _aux_read(void *dev, uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len)
{
    int8_t rslt;
    struct bmm150_dev *bmm150 = dev;

    /* Discarding the parameter id as it is redundant*/
    rslt = bmi160_aux_read(reg_addr, aux_data, len, bmm150->parent);

    return rslt;
}

/*wrapper function to match the signature of bmm150.write */
static int8_t _aux_write(void *dev, uint8_t id, uint8_t reg_addr, uint8_t *aux_data, uint16_t len)
{
    int8_t rslt;
    struct bmm150_dev *bmm150 = dev;

    /* Discarding the parameter id as it is redundant */
    rslt = bmi160_aux_write(reg_addr, aux_data, len, bmm150->parent);

    return rslt;
}

static void bmm150_create(struct bmi160_dev *bmi160)
{
    struct bmm150_dev *bmm150 = rt_calloc(1, sizeof(struct bmm150_dev));

    bmm150->parent = bmi160;
    bmi160->aux_dev = bmm150;

    /* Configure device structure for auxiliary sensor parameter */
    bmi160->aux_cfg.aux_sensor_enable = 1;                     // auxiliary sensor enable
    bmi160->aux_cfg.aux_i2c_addr = BMI160_AUX_BMM150_I2C_ADDR; // auxiliary sensor address
    bmi160->aux_cfg.manual_enable = 1;                         // setup mode enable
    bmi160->aux_cfg.aux_rd_burst_len = 2;                      // burst read of 2 byte

    /* Configure the BMM150 device structure by
    mapping _aux_read and _aux_write */
    bmm150->read = _aux_read;
    bmm150->write = _aux_write;
    bmm150->dev_id = BMM150_DEFAULT_I2C_ADDRESS;
    /* Ensure that sensor.aux_cfg.aux_i2c_addr = bmm150.id
       for proper sensor operation */
    bmm150->delay_ms = _delay_ms;
    bmm150->intf = BMM150_I2C_INTF;

    /* Initialize the auxiliary sensor interface */
    bmi160_aux_init(bmi160);

    /* Initialising the bmm150 sensor */
    bmm150_init(bmm150);

    /* Set the power mode and preset mode to enable Mag data sampling */
    bmm150->settings.pwr_mode = BMM150_NORMAL_MODE;
    bmm150_set_op_mode(bmm150);

    bmm150->settings.preset_mode = BMM150_PRESETMODE_LOWPOWER;
    bmm150_set_presetmode(bmm150);
}

#endif

static void _fifo_init(struct bmi160_dev *dev)
{
#define fifo_length FIFO_FRAME_CNT * 7
    /* Modify the FIFO buffer instance and link to the device instance */
    struct bmi160_fifo_frame *fifo = rt_malloc(
        sizeof(struct bmi160_fifo_frame) + fifo_length * sizeof(uint8_t));

    fifo->data = (uint8_t *)(fifo + 1);
    fifo->length = fifo_length;
    dev->fifo = fifo;
#undef fifo_length
}

static int rt_bmi160_sensor_register(void)
{
    static rt_uint8_t bmi160_dev_register_passed = 0;
    if (bmi160_dev_register_passed != 0)
        return RT_EOK;

    /* construct device */
    bmi160_acce_dev = rt_device_create(RT_Object_Class_Device, 0);
    if (bmi160_acce_dev == RT_NULL)
    {
        rt_kprintf("error happened when create bmi160 acce device\n");
        return RT_ENOMEM;
    }

    bmi160_gyro_dev = rt_device_create(RT_Object_Class_Device, 0);
    if (bmi160_gyro_dev == RT_NULL)
    {
        rt_kprintf("error happened when create bmi160 gyro device\n");
        return RT_ENOMEM;
    }

    /* set device function pointer */
    bmi160_acce_dev->init = RT_NULL;
    bmi160_acce_dev->open = rt_bmi160_acce_open;
    bmi160_acce_dev->close = rt_bmi160_acce_close;
    bmi160_acce_dev->read = rt_bmi160_acce_read;
    bmi160_acce_dev->write = rt_bmi160_acce_write;
    bmi160_acce_dev->control = rt_bmi160_control;

    bmi160_gyro_dev->init = RT_NULL;
    bmi160_gyro_dev->open = rt_bmi160_gyro_open;
    bmi160_gyro_dev->close = rt_bmi160_gyro_close;
    bmi160_gyro_dev->read = rt_bmi160_gyro_read;
    bmi160_gyro_dev->write = rt_bmi160_gyro_write;
    bmi160_gyro_dev->control = rt_bmi160_control;

    /* register device */
    rt_err_t rslt = rt_device_register(bmi160_acce_dev, BMI160_ACCE_NAME, RT_DEVICE_FLAG_RDWR);
    if (rslt != RT_EOK)
    {
        LOG_E("error happened when register bmi160 acce device, rslt = %d\n", rslt);
        return rslt;
    }

    rslt = rt_device_register(bmi160_gyro_dev, BMI160_GYRO_NAME, RT_DEVICE_FLAG_RDWR);
    if (rslt != RT_EOK)
    {
        LOG_E("error happened when register bmi160 gyro device, rslt = %d\n", rslt);
        return rslt;
    }

    bmi160_dev_register_passed = 1;

    return RT_EOK;
}

int start_bmi160(void)
{
    int8_t rslt = BMI160_OK;

    rslt = rt_bmi160_sensor_register();
    if (rslt != RT_EOK)
        return rslt;
#ifdef BMI160_USING_SPI
    rslt = bmi160_spi_msp_init();
#else
    rslt = bmi160_i2c_msp_init();
#endif
    if (rslt != RT_EOK)
        return rslt;

    bmi160_device = rt_calloc(1, sizeof(struct bmi160_dev));
    if (bmi160_device == RT_NULL)
    {
        LOG_E("bmi160 dev memory allocation failed");
        return RT_ENOMEM;
    }

#ifdef BMI160_USING_I2C
    bmi160_device->id = BMI160_I2C_ADDRESS; /* BMI160 I2C device address */
    bmi160_device->interface = BMI160_I2C_INTF;
    bmi160_device->bus = &bmi160_i2c_handle;
    bmi160_device->read = _i2c_read_reg;
    bmi160_device->write = _i2c_write_reg;
    bmi160_device->delay_ms = _delay_ms;
#else
    bmi160_device->interface = BMI160_SPI_INTF;
    bmi160_device->bus = &bmi160_spi_handle;
    bmi160_device->read = bmi160_spi_read_reg;
    bmi160_device->write = bmi160_spi_write_reg;
    bmi160_device->delay_ms = _delay_ms;
#endif

    rslt = bmi160_init(bmi160_device);
    if (rslt != BMI160_OK)
    {
        rt_kprintf("bmi160 init failed, chip id = 0x%02x, which expect 0x%02x or 0x%02x\n", bmi160_device->chip_id, BMI160_CHIP_ID, BMX160_CHIP_ID);
        rt_free(bmi160_device);
        return rslt;
    }
    rt_kprintf("bmi160 init success, chip id read 0x%02x is correct\n", bmi160_device->chip_id);

    rslt = bmi160_soft_reset(bmi160_device);
    if (rslt != BMI160_OK)
    {
        rt_kprintf("bmi160 soft reset failed");
        rt_free(bmi160_device);
        return rslt;
    }

    /* Select the Output data rate, range of accelerometer sensor */
    bmi160_device->accel_cfg.odr = BMI160_ACCEL_ODR_800HZ;
    bmi160_device->accel_cfg.range = BMI160_ACCEL_RANGE_2G;
    bmi160_device->accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;

    /* Select the power mode of accelerometer sensor */
    bmi160_device->accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;

    /* Select the Output data rate, range of Gyroscope sensor */
    bmi160_device->gyro_cfg.odr = BMI160_GYRO_ODR_800HZ;
    bmi160_device->gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
    bmi160_device->gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;

    /* Select the power mode of Gyroscope sensor */
    bmi160_device->gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;

    /* Set the sensor configuration */
    rslt = bmi160_set_sens_conf(bmi160_device);

    /* Select the power mode */
    bmi160_device->accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
    bmi160_device->gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;

    bmi160_set_power_mode(bmi160_device);

    _fifo_init(bmi160_device);

#ifdef BMX160_USING_MAG
    if (hdev->chip_id == BMX160_CHIP_ID) /* BMX160 */
    {
        bmm150_create(bmi160_device);
    }
#endif

    return RT_EOK;
}

static rt_err_t _set_odr(rt_device_t sensor, struct bmi160_dev *_bmi160_dev, rt_uint16_t odr)
{
    if (sensor == RT_NULL || _bmi160_dev == RT_NULL)
        return RT_EINVAL;

    if (sensor == bmi160_acce_dev)
    {
        const static struct odr_node tab[] =
            {
                {25, BMI160_ACCEL_ODR_25HZ},
                {50, BMI160_ACCEL_ODR_50HZ},
                {100, BMI160_ACCEL_ODR_100HZ},
                {200, BMI160_ACCEL_ODR_200HZ},
                {400, BMI160_ACCEL_ODR_400HZ},
                {800, BMI160_ACCEL_ODR_800HZ},
                {0, BMI160_ACCEL_ODR_1600HZ}};

        _bmi160_dev->accel_cfg.odr = range_find_mask(tab, odr);
        /* Set the desired configurations to the sensor */
        bmi160_set_sens_conf(_bmi160_dev);
        return RT_EOK;
    }

    if (sensor == bmi160_gyro_dev)
    {
        const static struct odr_node tab[] =
            {
                {25, BMI160_GYRO_ODR_25HZ},
                {50, BMI160_GYRO_ODR_50HZ},
                {100, BMI160_GYRO_ODR_100HZ},
                {200, BMI160_GYRO_ODR_200HZ},
                {400, BMI160_GYRO_ODR_400HZ},
                {800, BMI160_GYRO_ODR_800HZ},
                {1600, BMI160_GYRO_ODR_1600HZ},
                {0, BMI160_GYRO_ODR_3200HZ}};

        _bmi160_dev->gyro_cfg.odr = range_find_mask(tab, odr);
        /* Set the desired configurations to the sensor */
        bmi160_set_sens_conf(_bmi160_dev);
        return RT_EOK;
    }
#ifdef BMX160_USING_MAG
    if (sensor == bmx160_mag_dev)
    {
        struct bmm150_dev *bmm150 = _bmi160_dev->aux_dev;
        const static struct odr_node tab[] =
            {
                {2, BMM150_DATA_RATE_02HZ},
                {6, BMM150_DATA_RATE_06HZ},
                {8, BMM150_DATA_RATE_08HZ},
                {10, BMM150_DATA_RATE_10HZ},
                {15, BMM150_DATA_RATE_15HZ},
                {20, BMM150_DATA_RATE_20HZ},
                {25, BMM150_DATA_RATE_25HZ},
                {0, BMM150_DATA_RATE_30HZ},
            };

        bmm150->settings.data_rate = range_find_mask(tab, odr);
        bmm150_set_sensor_settings(BMM150_DATA_RATE_SEL, bmm150);
    }
#endif

    return RT_EOK;
}

static rt_err_t _set_range(rt_device_t sensor, struct bmi160_dev *_bmi160_dev, rt_uint16_t range)
{
    if (sensor == RT_NULL || _bmi160_dev == RT_NULL)
        return RT_EINVAL;

    if (sensor == bmi160_acce_dev)
    {
        const static struct odr_node tab[] =
            {
                {2000, BMI160_ACCEL_RANGE_2G},
                {4000, BMI160_ACCEL_RANGE_4G},
                {8000, BMI160_ACCEL_RANGE_8G},
                {0, BMI160_ACCEL_RANGE_16G}};

        _bmi160_dev->accel_cfg.range = range_find_mask(tab, range);
        /* Set the sensor configuration */
        bmi160_set_sens_conf(_bmi160_dev);

        return RT_EOK;
    }
    else if (sensor == bmi160_gyro_dev)
    {
        const static struct odr_node tab[] =
            {
                {125, BMI160_GYRO_RANGE_125_DPS},
                {250, BMI160_GYRO_RANGE_250_DPS},
                {500, BMI160_GYRO_RANGE_500_DPS},
                {1000, BMI160_GYRO_RANGE_1000_DPS},
                {0, BMI160_GYRO_RANGE_2000_DPS}};

        _bmi160_dev->accel_cfg.range = range_find_mask(tab, range);
        /* Set the sensor configuration */
        bmi160_set_sens_conf(_bmi160_dev);

        return RT_EOK;
    }

    return RT_ERROR;
}

static rt_err_t _set_power(rt_device_t sensor, struct bmi160_dev *_bmi160_dev, rt_uint8_t power)
{
    int idx;
    struct bmm150_dev *bmm150 = _bmi160_dev->aux_dev;
    uint8_t *const pwr_reg[3] =
        {
            &_bmi160_dev->accel_cfg.power,
            &_bmi160_dev->gyro_cfg.power,
            &bmm150->settings.pwr_mode};
    const uint8_t pwr_cfg[3][3] =
        {
            {BMI160_ACCEL_SUSPEND_MODE, BMI160_GYRO_SUSPEND_MODE, BMM150_SUSPEND_MODE},
            {BMI160_ACCEL_NORMAL_MODE, BMI160_GYRO_NORMAL_MODE, BMM150_NORMAL_MODE},
            {BMI160_ACCEL_LOWPOWER_MODE, BMI160_GYRO_NORMAL_MODE, BMM150_FORCED_MODE},
        };

    if (sensor == bmi160_acce_dev)
        idx = 0;
    else if (sensor == bmi160_gyro_dev)
        idx = 1;
    else if (sensor == bmx160_mag_dev)
        idx = 2;
    else
    {
        LOG_W("Unsupported sensor type");
        return -RT_ERROR;
    }

    switch (power)
    {
    case sensor_power_down: /* power down */
        *pwr_reg[idx] = pwr_cfg[0][idx];
        break;
    case sensor_power_normal: /* power normal */
        *pwr_reg[idx] = pwr_cfg[1][idx];
        break;
    case sensor_power_low: /* power low */
        *pwr_reg[idx] = pwr_cfg[2][idx];
        break;
    default:
        LOG_W("Unsupported mode, code is %d", power);
        return -RT_ERROR;
    }

    bmi160_set_power_mode(_bmi160_dev);
    bmm150_set_op_mode(bmm150);
    return RT_EOK;
}

static void _get_mag_config_fifo(struct bmi160_dev *hdev)
{
    /* Enter the data register of BMM150 to "auto_mode_addr" here it is 0x42 */
    uint8_t auto_mode_addr = 0x42;
    hdev->aux_cfg.aux_odr = BMI160_AUX_ODR_25HZ;
    bmi160_set_aux_auto_mode(&auto_mode_addr, hdev);

    /* Disable other FIFO settings */
    bmi160_set_fifo_config(BMI160_FIFO_CONFIG_1_MASK, BMI160_DISABLE, hdev);

    /* Enable the required FIFO settings */
    bmi160_set_fifo_config(BMI160_FIFO_AUX | BMI160_FIFO_HEADER, BMI160_ENABLE, hdev);
}

static rt_err_t _select_mode(rt_device_t sensor, struct bmi160_dev *_bmi160_dev, rt_uint32_t mode)
{
    int use_int = 0;
    struct bmi160_int_settg int_config;

    if (mode == sensor_mode_int)
    {
        use_int = 1;
        /* Select the Interrupt type */
        int_config.int_type = BMI160_ACC_GYRO_DATA_RDY_INT; // Choosing Gyro and Acce Data Ready interrupt
    }
    else if (mode == sensor_mode_fifo)
    {
        use_int = 1;
        /* Select the Interrupt channel/pin */
        int_config.int_channel = BMI160_INT_CHANNEL_1; // Interrupt channel/pin 1
        /* Select the Interrupt type */
        int_config.int_type = BMI160_ACC_GYRO_FIFO_FULL_INT; // Choosing Gyro and Acce FIFO Full interrupt

        if (sensor == bmx160_mag_dev)
        {
            _get_mag_config_fifo(_bmi160_dev);
        }
        else
        {
            bmi160_set_fifo_config(BMI160_FIFO_GYRO | BMI160_FIFO_ACCEL | BMI160_FIFO_HEADER | BMI160_FIFO_TIME, BMI160_ENABLE, _bmi160_dev);
        }
    }

    if (use_int)
    {
        /* Select the Interrupt channel/pin */
        int_config.int_channel = BMI160_INT_CHANNEL_1; // Interrupt channel/pin 1

        /* Select the interrupt channel/pin settings */
        int_config.int_pin_settg.output_en = BMI160_ENABLE;         // Enabling interrupt pins to act as output pin
        int_config.int_pin_settg.output_mode = BMI160_DISABLE;      // Choosing push-pull mode for interrupt pin
        int_config.int_pin_settg.output_type = BMI160_ENABLE;       // Choosing active high output
        int_config.int_pin_settg.edge_ctrl = BMI160_ENABLE;         // Choosing edge triggered output
        int_config.int_pin_settg.input_en = BMI160_DISABLE;         // Disabling interrupt pin to act as input
        int_config.int_pin_settg.latch_dur = BMI160_LATCH_DUR_NONE; // non-latched output

        /* Set the interrupt mode */
        bmi160_set_int_config(&int_config, _bmi160_dev); /* sensor is an instance of the structure bmi160_dev */
    }
    return RT_EOK;
}

static float acce_sensitivity(uint8_t acce_range)
{
    float val;
    switch (acce_range)
    {
    case BMI160_ACCEL_RANGE_2G:
        val = 16384;
        break;
    case BMI160_ACCEL_RANGE_4G:
        val = 8192;
        break;
    case BMI160_ACCEL_RANGE_8G:
        val = 4096;
        break;
    case BMI160_ACCEL_RANGE_16G:
        val = 2048;
        break;
    default:
        val = -1;
        break;
    }
    return val;
}

static float gyro_sensitivity(uint8_t gyro_range)
{
    float val;
    switch (gyro_range)
    {
    case BMI160_GYRO_RANGE_125_DPS:
        val = 262.4f;
        break;
    case BMI160_GYRO_RANGE_250_DPS:
        val = 131.2f;
        break;
    case BMI160_GYRO_RANGE_500_DPS:
        val = 65.6f;
        break;
    case BMI160_GYRO_RANGE_1000_DPS:
        val = 32.8f;
        break;
    case BMI160_GYRO_RANGE_2000_DPS:
        val = 16.4f;
        break;
    default:
        val = -1;
        break;
    }
    return val;
}

/// @brief 将bmi160传感器数据拷贝到通用传感器数据结构体buf
/// @param dst
/// @param src
/// @param sensor 传感器对象，这里用来区分是acce还是gyro
static void _copy_data(struct sensor_data *dst, struct bmi160_sensor_data *src, rt_device_t sensor)
{
    if (!sensor || !dst || !src || !bmi160_device)
        return;

    if (sensor == bmi160_acce_dev)
    {
        float sensitivity = acce_sensitivity(bmi160_device->accel_cfg.range);
        dst->data.acce.x = src->x / sensitivity * 1000; /* 此时dst->data.acce的三轴加速度单位mg */
        dst->data.acce.y = src->y / sensitivity * 1000;
        dst->data.acce.z = src->z / sensitivity * 1000;
    }
    else if (sensor == bmi160_gyro_dev)
    {
        float sensitivity = gyro_sensitivity(bmi160_device->gyro_cfg.range);
        dst->data.gyro.x = src->x / sensitivity * 1000; /* 此时dst->data.gyro的三轴角速度单位mdps */
        dst->data.gyro.y = src->y / sensitivity * 1000;
        dst->data.gyro.z = src->z / sensitivity * 1000;
    }

    dst->timestamp = rt_tick_get();
}

/// @brief 获取一份磁力计数据
/// @param dst
static void _get_mag_data(struct sensor_data *dst)
{
    if (!bmi160_device || !dst)
        return;

    struct bmm150_dev *bmm150 = bmi160_device->aux_dev;
    bmm150_read_mag_data(bmm150);

    dst->data.mag.x = bmm150->data.x * 0.3f / 1000; /* 此时dst->data.mag的三轴磁场强度mT */
    dst->data.mag.y = bmm150->data.y * 0.3f / 1000;
    dst->data.mag.z = bmm150->data.z * 0.3f / 1000;
}

/// @brief 获取一份三轴加速度/陀螺仪/磁力计传感器数据
/// @param sensor
/// @param buf
/// @return
static int _get_data(rt_device_t sensor, void *buf)
{
    if (!buf || !sensor || !bmi160_device)
        return RT_EINVAL;

    struct sensor_data *data = (sensor_data_t *)buf;
    struct bmi160_sensor_data acce_data, gyro_data;

    if (sensor == bmx160_mag_dev && bmi160_device->chip_id == BMX160_CHIP_ID)
    {
        _get_mag_data(data);
        return RT_EOK;
    }

    bmi160_get_sensor_data(BMI160_ACCEL_SEL | BMI160_GYRO_SEL, &acce_data, &gyro_data, bmi160_device);
    if (sensor == bmi160_acce_dev)
    {
        _copy_data(data, &acce_data, sensor);
    }
    else if (sensor == bmi160_gyro_dev)
    {
        _copy_data(data, &gyro_data, sensor);
    }

    return RT_EOK;
}

/// @brief 将bmm150磁力计数据存储到bmi160设备结构体内
/// @param hdev
/// @param aux_data
/// @param data
/// @param len
static void _extract_mag(struct bmi160_dev *hdev,
                         struct bmi160_aux_data *aux_data, struct bmi160_sensor_data *data, uint8_t len)
{
    int i;
    struct bmm150_dev *bmm150 = hdev->aux_dev;

    for (i = 0; i < len; i++)
    {
        /* Compensated mag data using BMM150 API */
        int sre = bmm150_aux_mag_data(aux_data[i].data, bmm150);

        /* Copy the  Compensated mag data */
        if (sre == BMM150_OK)
        {
            data[i].x = bmm150->data.x;
            data[i].y = bmm150->data.y;
            data[i].z = bmm150->data.z;
        }
    }
}

/// @brief 获取8个FIFO帧，每隔FIFO帧包含加速度、陀螺仪以及帧头，可以在初始化时配置FIFO帧格式
/// @param sensor
/// @param buf
/// @param len
/// @return
static int _get_fifo_data(rt_device_t sensor, void *buf, rt_size_t len)
{
    if (!sensor || !buf || !bmi160_device)
        return RT_EINVAL;

    uint8_t frame_len = FIFO_FRAME_CNT;
    struct sensor_data *data = buf;
    struct bmi160_sensor_data buffer[FIFO_FRAME_CNT]; // 32 data frames

    if (bmi160_get_fifo_data(bmi160_device) != BMI160_OK)
        return 0;

    if (sensor == bmi160_acce_dev)
    {
        bmi160_extract_accel(buffer, &frame_len, bmi160_device);
    }
    else if (sensor == bmi160_gyro_dev)
    {
        bmi160_extract_gyro(buffer, &frame_len, bmi160_device);
    }
    else if (sensor == bmx160_mag_dev)
    {
        struct bmi160_aux_data aux_data[FIFO_FRAME_CNT];
        bmi160_extract_aux(aux_data, &frame_len, bmi160_device);
        _extract_mag(bmi160_device, aux_data, buffer, frame_len);
    }

    len = len < frame_len ? len : frame_len;
    for (int i = 0; i < len; i++)
    {
        _copy_data(data + i, buffer + i, sensor);
    }
    return len;
}

/// @brief
/// @param dev
/// @param oflag
/// @return
static rt_err_t rt_bmi160_acce_open(rt_device_t dev, rt_uint16_t oflag)
{
    if (dev == RT_NULL || dev != bmi160_acce_dev || bmi160_device == RT_NULL)
    {
        LOG_E("bmi160 accel open failed for device pointer not correct\n");
        return RT_ERROR;
    }

    rt_err_t rslt = RT_EOK;
    if (bmi160_device->accel_cfg.power == BMI160_ACCEL_PMU_SUSPEND)
        rslt = _set_power(dev, bmi160_device, sensor_power_normal);

    return rslt;
}

static rt_err_t rt_bmi160_acce_close(rt_device_t dev)
{
    if (dev == RT_NULL || dev != bmi160_acce_dev || bmi160_device == RT_NULL)
    {
        LOG_E("bmi160 accel close failed for device pointer not correct\n");
        return RT_ERROR;
    }

    _set_power(dev, bmi160_device, sensor_power_down);

    return RT_EOK;
}

/// @brief 读取加速度数据
/// @param dev
/// @param pos
/// @param buffer
/// @param size 只有FIFO模式才有意义，表示读取几个FIFO帧，每个FIFO帧有一组加速度数据
/// @return 读取字节数
static rt_size_t rt_bmi160_acce_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi160_acce_dev || bmi160_device == RT_NULL)
    {
        LOG_E("bmi160 accel read failed for device pointer not correct\n");
        return RT_ERROR;
    }

    rt_size_t rd_size = 0;

    if (read_mode == sensor_mode_normal)
    {
        _get_data(dev, buffer);
        rd_size = sizeof(struct bmi160_sensor_data);
    }

    else if (read_mode == sensor_mode_fifo)
    {
        _get_fifo_data(dev, buffer, size);
        rd_size = size * sizeof(struct bmi160_sensor_data);
    }

    return rd_size;
}

static rt_size_t rt_bmi160_acce_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi160_acce_dev || bmi160_device == RT_NULL || buffer == RT_NULL)
    {
        LOG_E("bmi160 accel write failed for device pointer not correct\n");
        return RT_ERROR;
    }

#ifdef BMI160_USING_I2C
    HAL_I2C_Mem_Write(&bmi160_i2c_handle, BMI160_I2C_ADDRESS, pos, I2C_MEMADD_SIZE_8BIT, (uint8_t *)buffer, size, size);
#else
    bmi160_spi_write_reg(RT_NULL, 0, pos, (uint8_t *)buffer, size);
#endif

    return size;
}

static rt_err_t rt_bmi160_control(rt_device_t dev, int cmd, void *args)
{
    rt_uint32_t *val = (rt_uint32_t *)args;
    rt_err_t rslt = RT_EOK;

    switch (cmd)
    {
    case get_id:
        *(rt_uint8_t *)args = bmi160_device->chip_id;
        break;

    case set_odr:
        rslt = _set_odr(dev, bmi160_device, *val);
        break;

    case set_range:
        rslt = _set_range(dev, bmi160_device, (rt_uint32_t)args);
        break;

    case set_powermode:
        rslt = _set_power(dev, bmi160_device, *val);
        break;

    case set_rdmode:
        rslt = _select_mode(dev, bmi160_device, (rt_uint32_t)args);
        break;

    default:
        break;
    }

    return rslt;
}

static rt_err_t rt_bmi160_gyro_open(rt_device_t dev, rt_uint16_t oflag)
{
    if (dev == RT_NULL || dev != bmi160_gyro_dev || bmi160_device == RT_NULL)
    {
        LOG_E("bmi160 gyro open failed for device pointer not correct\n");
        return RT_ERROR;
    }

    rt_err_t rslt = RT_EOK;
    if (bmi160_device->gyro_cfg.power == BMI160_GYRO_PMU_SUSPEND)
        rslt = _set_power(dev, bmi160_device, sensor_power_normal);

    return rslt;
}

static rt_err_t rt_bmi160_gyro_close(rt_device_t dev)
{
    if (dev == RT_NULL || dev != bmi160_gyro_dev || bmi160_device == RT_NULL)
    {
        LOG_E("bmi160 gyro close failed for device pointer not correct\n");
        return RT_ERROR;
    }

    _set_power(dev, bmi160_device, sensor_power_down);

    return RT_EOK;
}

static rt_size_t rt_bmi160_gyro_read(rt_device_t dev, rt_off_t pos, void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi160_gyro_dev || bmi160_device == RT_NULL)
    {
        LOG_E("bmi160 gyro read failed for device pointer not correct\n");
        return RT_ERROR;
    }

    rt_size_t rd_size = 0;

    if (read_mode == sensor_mode_normal)
    {
        _get_data(dev, buffer);
        rd_size = sizeof(struct bmi160_sensor_data);
    }

    else if (read_mode == sensor_mode_fifo)
    {
        _get_fifo_data(dev, buffer, size);
        rd_size = size * sizeof(struct bmi160_sensor_data);
    }

    return rd_size;
}

static rt_size_t rt_bmi160_gyro_write(rt_device_t dev, rt_off_t pos, const void *buffer, rt_size_t size)
{
    if (dev == RT_NULL || dev != bmi160_acce_dev || bmi160_device == RT_NULL || buffer == RT_NULL)
    {
        LOG_E("bmi160 gyro write failed for device pointer not correct\n");
        return RT_ERROR;
    }

#ifdef BMI160_USING_I2C
    HAL_I2C_Mem_Write(&bmi160_i2c_handle, BMI160_I2C_ADDRESS, pos, I2C_MEMADD_SIZE_8BIT, (uint8_t *)buffer, size, size);
#else
    bmi160_spi_write_reg(RT_NULL, 0, pos, (uint8_t *)buffer, size);
#endif

    return size;
}

#endif

int bmi160_sensor_sample(void)
{
    rt_device_t bmi160_acce = rt_device_find(BMI160_ACCE_NAME);
    if (bmi160_acce == RT_NULL)
    {
        LOG_E("find %s failed\n", BMI160_ACCE_NAME);
        return RT_ERROR;
    }

    rt_device_t bmi160_gyro = rt_device_find(BMI160_GYRO_NAME);
    if (bmi160_gyro == RT_NULL)
    {
        LOG_E("find %s failed\n", BMI160_GYRO_NAME);
        return RT_ERROR;
    }

    sensor_data_t imu_acce, imu_gyro;

    for (int i = 0; i < 5; i++)
    {
        rt_device_read(bmi160_acce, 0, &imu_acce, 1);
        rt_kprintf("bmi160 raw acce data = [%d, %d, %d]mg\n",
                   imu_acce.data.acce.x,
                   imu_acce.data.acce.y,
                   imu_acce.data.acce.z);

        rt_device_read(bmi160_gyro, 0, &imu_gyro, 1);
        rt_kprintf("bmi160 raw gyro data = [%d, %d, %d]mdps\n",
                   imu_acce.data.gyro.x,
                   imu_acce.data.gyro.y,
                   imu_acce.data.gyro.z);
    }

    return RT_EOK;
}
MSH_CMD_EXPORT(bmi160_sensor_sample, "bmi160_sensor_device_sample");

int bmi160_read_id(void)
{
    struct bmi160_pmu_status s;
    bmi160_get_power_mode(&s, bmi160_device);
    rt_kprintf("bmi160 power status: %d, %d, %d\n",
               s.accel_pmu_status,
               s.gyro_pmu_status,
               s.aux_pmu_status);

    uint8_t buf[5];
    bmi160_spi_read_reg(NULL, 0, BMI160_CHIP_ID_ADDR | BMI160_SPI_RD_MASK, buf, 2);
    rt_kprintf("read id : %02x %02x\n", buf[0], buf[1]);
    return 0;
}
MSH_CMD_EXPORT(bmi160_read_id, "read bmi160 id");